#include <stdlib.h>
#include <stdio.h>

#include "simpletlv.h"


/*************************************************************************
 *  Pack TLVs allocating space
 *************************************************************************/
int tlv_pack(char** pack, int* packlen, int tag, int len, const char* value){
	char *x;
	int oldlen=*packlen;
	*packlen = *packlen+len+1+1;
	if(!(x=(char*)realloc(*pack, *packlen)))
		return 1;
	x[oldlen] = (char)tag;
	x[oldlen+1] = (char)len;	
	memcpy(x+oldlen+2,value, len);
	*pack = x;
	return 0;

}

int unpack_peekszlenght(char* pack){
	return pack[1];
}

/*************************************************************************
 * Unpack TLVs  deallocating space - the caller has to free val
 *************************************************************************/
int tlv_unpack(char** pack, int* packlen, int* tag, int* len, char** val ){
	int i, pos;
	char* x;

	if(*val){
		//MEM LEAK alert!
	}

	for(pos=1 ; (i=(*pack)[pos]+pos+1+1)< *packlen ; pos=i)

	*tag=(*pack)[pos-1];
	*len=(*pack)[pos];
	if(!(*val=malloc(*len)))
		return 1;
	*packlen-=(*len+1+1);
	memcpy(*val,(*pack)+pos+1,*len);
	if(!*packlen){
		free(*pack);
		*pack=0;
	}
	else if(!(x=realloc(*pack,*packlen))){
		return 1;
	}
	*pack=x;
	return 0;
}

/*************************************************************************
 * Unpack TLVs  deallocating space - the caller has to free val
 *************************************************************************/
int tlv_show(char* x, int y, FILE* fp){
	int i;
	for(i=0;i<y;i++)
		(x[i]>=32 && x[i]<=126)?fprintf(fp, "%c ", x[i]):fprintf(fp, "0x%0.2x ", x[i]);
	fprintf(fp, "\n");
}
int tlv_showpacked(char* pack, int packlen, FILE* fp){
	int i, pos;
	char* x;

	for(pos=1 ; (i=pack[pos]+pos+1+1)< packlen ; pos=i){
		fprintf(fp,"t:%d l:%d v: ",pack[pos-1], pack[pos]);tlv_show(pack+pos+1, pack[pos],fp);
	}
	fprintf(fp,"t:%d l:%d v: ",pack[pos-1], pack[pos]);tlv_show(pack+pos+1, pack[pos],fp);
}
